#include "imgA4.hpp"
#include "aligntarget.hpp"
#include "Thread.h"
#include <rthw.h>
#include <rtthread.h>
#include "artVcontact.hpp"
#include "anodt.hpp"
#include "tsp_solve.hpp"

using namespace artVcontact;

constexpr uint16_t Hesha[]{
#include "DM.txt"
};

int32_t imgpoc_time;
int32_t imgstart_time;

unsigned char imgpoc_status=0;//处理状态机

uint8_t picture_display_flag=0;


float coord_threshold=25;
float aim_threshold=22;



static void ImgProcThreadEntry(void*) 
{
   	using namespace imgProc::imgA4; 
	using namespace imgProc::align_target;
	using namespace tsp_solve;
	guidisplayimg(&Hesha[0], 320,240 , 320, 240);
    for(;;)
    {
		bool visualize = 0; 
		if(gpio_get(D4)==0)
		visualize=1;
		imgstart_time = rt_tick_get();//Tim
		uint8_t* img = mt9v03x_csi_image_take();
		if(imgpoc_status==0)
		{
			
			unsigned char coord_num;
			ips200_displayimage032_zoom1((uint8_t*)img,MT9V03X_CSI_W,MT9V03X_CSI_H,0,0,188,120);//可视化显示灰度图
			coord_num=Coord_scan((uint8_t(*)[MT9V03X_CSI_W])img,700,500,coord_threshold,visualize);
			
			if(coord_num!=0&&coord_num>3)
			{
				static unsigned char coord_num_check=0,checkcnt=0;
				if(coord_num_check!=coord_num)
				{
					checkcnt=0;
					coord_num_check=coord_num;
				}
				else
				{
					checkcnt++;
				}
				if(checkcnt>=2)
				{
					pick_coords_num=coord_num;
					imgpoc_status++;
					buzzflag=2;
					clock_startflag=1;
					rt_sem_release(coord_ok_sem);
				}
			} 
		}
		if(imgpoc_status==1)
		{
			
			st_quad_pixle  Pcoord;
			uint8_t result;
			
			uint8_t img_small [ALIG_H*ALIG_W];
			for(int i=0;i<ALIG_H;i++)
			{
				for(int j=0;j<ALIG_W;j++)
				{
					img_small[i*188+j]=img[i*2*376+j*2];
				}
			}
			if(visualize)ips200_displayimage032_zoom1((uint8_t*)img_small,ALIG_W,ALIG_H,0,0,188,120);
			 result=find_quads((uint8_t(*)[ALIG_W])img_small,aim_threshold,&Pcoord,visualize);
			if(result)
			{
				if(Xstatus==2&&abs(Car_Encoder_SumX-Car_DistanceX)<15000)//如果在路程闭环阶段视野中出现方块时，直接切到矫正阶段30cm
				{
					Xstatus=0;
					Car_Encoder_SumX=0;
					Car_DistanceX=0;
					Car_SpeedX=0;
					buzzflag=6;
					rt_sem_release(movx_sem);
				}
				Set_RealAim_XY(Pcoord.average_x,Pcoord.average_y);
			}
			// else
			// {
			// 	rt_kprintf("NO\n");
			// }
			
		}
		if(visualize)
		{
			picture_display_flag=1;
			ips200_showstr(188,14,"YA");
			ips200_showfloat(204,14,yaw,3,2);
  		}
		else 
		{
			if(picture_display_flag==1)
			{
				picture_display_flag=0;
				guidisplayimg(&Hesha[0], 320,240 , 320, 240);
			}
		}
		mt9v03x_csi_image_release();  
		imgpoc_time=rt_tick_get()-imgstart_time;//Tim
		char ipsstr[20];
		sprintf(ipsstr,"T:%2d  ",imgpoc_time);
		ips200_showstr(188,13,ipsstr);
    }//for(;;)
}





rtthread::Thread ImgProcThread(ImgProcThreadEntry, NULL, 4096*16, 13, 1000, "ImgProcThread");


